#
# CSE466 Final Project
# 
# PS3 Controller for the blimp
#
# (c) Dustin Richmond, Alex K. Horton, James Youngquist
#

import serial
import pygame
import pprint
import math
import protocol
import struct
import numpy as np

pp = pprint.PrettyPrinter(width = 200).pprint

dl = protocol.Datalink("/dev/ttyUSB0")

def get_sensor_vals():
    new_data = np.array(struct.unpack("hhhhhh", ''.join(dl.receive())[:12])[1:])
    return new_data
    
def get_motor_vals(axes):
    l_h = axes[0]               # left thumb horizontal
    l_v = -axes[1]               # left thumb vertical
    
    theta = math.atan2(l_v, l_h)
    x = math.cos(theta)
    y = math.sin(theta)

#    if l_h > 0 and l_v > 0:
    motor_right = l_v - l_h
    motor_left = l_v + l_h

    if motor_right < -1: motor_right = -1.0
    if motor_right > 1: motor_right = 1.0
    if motor_left < -1: motor_left = -1.0
    if motor_left > 1: motor_left = 1.0

    r_v = -axes[3]
    dead_zone = 0.3
    if r_v < -dead_zone:
        motor_vert = -1
    elif r_v > dead_zone:
        motor_vert = 1
    else:
        motor_vert = 0
        
    return (int(motor_left * 120), int(motor_right * 120), motor_vert)


def set_motor(S):
    dl.send(struct.pack("bbbb", 1, S[0], S[1], S[2]))
    return

if __name__ == "__main__":
    pygame.init()
    js = pygame.joystick.Joystick(0)
    js.init()
    n_axes = js.get_numaxes()
    n_buttons = js.get_numbuttons()

    clock = pygame.time.Clock()

    while True:
        #for event in pygame.event.get():
        pygame.event.pump()
        axes = [js.get_axis(i) for i in (0,1,2,3)]
        vals = get_motor_vals(axes)
        print vals
        set_motor(vals)
        get_sensor_vals()

